/*
     Description: Xilinx Virtual Cable Server for ESP32

     See Licensing information at End of File.
*/

#include <WiFi.h>
#include <lwip/sockets.h>
#include <lwip/netdb.h>
#include <cstdio>
#include <cstdlib>
#include <cstdint>
#include <cstring>
#include <memory>
#include <cstdlib>
#include <SSD1306Wire.h>

/*------引脚分配，可修改-------*/
//led引脚，低电平点亮
#define LED_blk 2 
//支持SSD1306主控 IIC接口oled屏幕
#define SDA 1 //oled iic SDA
#define SCL 0 //oled iic SCL
// 选择JTAG信号与GPIO的关联，注意不要与UART，I2C等功能冲突
// 填入GPIOx的编号
#define TMS_PIN  7 //TMS
#define TCK_PIN  6 //TCK
#define TDO_PIN  5 //TDO
#define TDI_PIN  4 //TDI
//打印调试信息
//#define PRINT_LOG_UART
/*------引脚分配，可修改-------*/

#define XVC_Port_Num 2542 //默认XVC端口号
char ip_string[20];
SSD1306Wire display(0x3c, SDA, SCL, GEOMETRY_128_32);
bool led_sta=1;
#define ERROR_JTAG_INIT_FAILED -1
#define ERROR_OK 1
#define JTAG_READ_TDO (digitalRead(TDO_PIN) & 1U)
#define BUFFER_SIZE 2048
#define JTAG_DELAY 1U // 软件延迟系数
//__attribute__((optimize("O3"))) __attribute__((always_inline))
std::uint8_t jtag_xfer(std::uint32_t n, std::uint8_t tms, std::uint8_t tdi) __attribute__((always_inline)) __attribute__((optimize("O3")));//高频函数声明内联+o3
std::uint8_t jtag_xfer(std::uint32_t n, std::uint8_t tms, std::uint8_t tdi)
{
    std::uint8_t tdo = 0;
    std::uint8_t tdi_tmp;
    std::uint8_t tms_tmp;
    std::uint8_t tdi_old = 0xF;
    std::uint8_t tms_old = 0xF;
    if (0)//TMS=00|FF
    {
        /* SPI-JTAG */
    }
    else
    {
        for (std::uint32_t i = 0; i < n; i++) {
            //negedge
            tms_tmp = tms & 1U;
            tdi_tmp = tdi & 1U;
            //若IO状态和上次相同，则跳过，优化速度
            if(tdi_old != tdi_tmp)
            {
                digitalWrite(TDI_PIN, tdi_tmp);
            }
            if (tms_old != tms_tmp)
            {
                digitalWrite(TMS_PIN, tms_tmp);
            }
            digitalWrite(TCK_PIN, LOW);
            tdi_old = tdi_tmp;
            tms_old = tms_tmp;
            //posedge
            digitalWrite(TCK_PIN, HIGH);
            tdo |= JTAG_READ_TDO << i;
            tms >>= 1;
            tdi >>= 1;
        }
    }
    return tdo;
}

static int jtag_init(void)
{
    pinMode(TDO_PIN, INPUT);
    pinMode(TDI_PIN, OUTPUT);
    pinMode(TCK_PIN, OUTPUT);
    pinMode(TMS_PIN, OUTPUT);

    digitalWrite(TDI_PIN, LOW);
    digitalWrite(TCK_PIN, LOW);
    digitalWrite(TMS_PIN, HIGH);

    return ERROR_OK;
}

static int sread(int fd, void* target, int len) {
    std::uint8_t *t = reinterpret_cast<std::uint8_t*>(target);
    while (len) {
        int r = read(fd, t, len);
        if (r <= 0)
            return r;
        t += r;
        len -= r;
    }
    return 1;
}

static constexpr const char* TAG = "XVC";

struct Socket
{
    int fd;
    Socket() : fd(-1) {}
    Socket(int fd) : fd(fd) {}
    Socket(int domain, int family, int protocol)
    {
        this->fd = socket(domain, family, protocol);
    }
    Socket(const Socket&) = delete;
    Socket(Socket&& rhs) : fd(rhs.fd) {
        rhs.fd = -1;
    }
    ~Socket() {
        this->release();
    }
    void release() {
        if (this->is_valid()) {
            closesocket(this->fd);
            this->fd = -1;
        }
    }
    int get() const {
        return this->fd;
    }

    Socket& operator=(Socket&& rhs) {
        this->fd = rhs.fd;
        rhs.fd = -1;
        return *this;
    }
    bool is_valid() const {
        return this->fd > 0;
    }
    operator int() const {
        return this->get();
    }
};

class XvcServer
{
    private:
        Socket listen_socket;
        Socket client_socket;
        std::uint8_t buffer[BUFFER_SIZE], result[1024];
    public:
        XvcServer(std::uint16_t port) {
            Socket sock(AF_INET, SOCK_STREAM, 0);
            {
                int value = 1;
                setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value));
            }

            sockaddr_in address;
            address.sin_addr.s_addr = INADDR_ANY;
            address.sin_port = htons(port);
            address.sin_family = AF_INET;

            if (bind(sock, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "Failed to bind socket.");
                #endif
            }
            if (listen(sock, 0) < 0) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "Failed to listen the socket.");
                #endif
            }
            #ifdef PRINT_LOG_UART
            ESP_LOGI(TAG, "Begin XVC Server. port=%d", port);
            #endif
            this->listen_socket = std::move(sock);
        }
        XvcServer(const XvcServer&) = delete;

        bool wait_connection()
        {
            fd_set conn;
            int maxfd = this->listen_socket.get();

            FD_ZERO(&conn);
            FD_SET(this->listen_socket.get(), &conn);

            fd_set read = conn, except = conn;
            int fd;

            if (select(maxfd + 1, &read, 0, &except, 0) < 0) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "select");
                #endif
                return false;
            }

            for (fd = 0; fd <= maxfd; ++fd) {
                if (FD_ISSET(fd, &read)) {
                    if (fd == this->listen_socket.get()) {
                        int newfd;
                        sockaddr_in address;
                        socklen_t nsize = sizeof(address);
                        newfd = accept(this->listen_socket.get(), reinterpret_cast<sockaddr*>(&address), &nsize);
                        #ifdef PRINT_LOG_UART
                        ESP_LOGI(TAG, "connection accepted - fd %d\n", newfd);
                        #endif
                        if (newfd < 0) {
                            #ifdef PRINT_LOG_UART
                            ESP_LOGE(TAG, "accept returned an error.");
                            #endif
                        } else {
                            if (newfd > maxfd) {
                                maxfd = newfd;
                            }
                            FD_SET(newfd, &conn);
                            this->client_socket = Socket(newfd);
                            //vivado连上了
                            display.drawString(64, 20, "XVC OK");
                            display.display();
                            return true;
                        }
                    }

                }
            }

            return false;
        }

        bool handle_data()
        {
            const char xvcInfo[] = "xvcServer_v1.0:2048\n";
            int fd = this->client_socket.get();

            std::uint8_t cmd[16];
            std::memset(cmd, 0, 16);

            if (sread(fd, cmd, 2) != 1)
                return false;

            if (memcmp(cmd, "ge", 2) == 0) {
                if (sread(fd, cmd, 6) != 1)
                    return 1;
                memcpy(result, xvcInfo, strlen(xvcInfo));
                if (write(fd, result, strlen(xvcInfo)) != strlen(xvcInfo)) {
                    #ifdef PRINT_LOG_UART
                    ESP_LOGE(TAG, "write");
                    #endif
                    return 1;
                }
                #ifdef PRINT_LOG_UART
                ESP_LOGD(TAG, "%u : Received command: 'getinfo'\n", (int)time(NULL));
                ESP_LOGD(TAG, "\t Replied with %s\n", xvcInfo);
                #endif
                return true;
            } else if (memcmp(cmd, "se", 2) == 0) {
                if (sread(fd, cmd, 9) != 1)
                    return 1;
                memcpy(result, cmd + 5, 4);
                if (write(fd, result, 4) != 4) {
                    #ifdef PRINT_LOG_UART
                    ESP_LOGE(TAG, "write");
                    #endif
                    return 1;
                }
                #ifdef PRINT_LOG_UART
                ESP_LOGD(TAG, "%u : Received command: 'settck'\n", (int)time(NULL));
                ESP_LOGD(TAG, "\t Replied with '%.*s'\n\n", 4, cmd + 5);
                #endif
                return true;
            } else if (memcmp(cmd, "sh", 2) == 0) {
                if (sread(fd, cmd, 4) != 1)
                    return false;
                #ifdef PRINT_LOG_UART
                ESP_LOGD(TAG, "%u : Received command: 'shift'\n", (int)time(NULL));
                #endif
            } else {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "invalid cmd '%s'\n", cmd);
                #endif
                return false;
            }

            std::uint32_t len;
            if (sread(fd, &len, 4) != 1) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "reading length failed\n");
                #endif
                return false;
            }

            std::uint32_t nr_bytes = (len + 7) / 8;
            if (nr_bytes * 2 > BUFFER_SIZE) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "buffer size exceeded\n");
                #endif
                return false;
            }

            if (sread(fd, buffer, nr_bytes * 2) != 1) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "reading data failed\n");
                #endif
                return false;
            }
            #ifdef PRINT_LOG_UART
            ESP_LOGD(TAG, "Number of Bits  : %d\n", len);
            ESP_LOGD(TAG, "Number of Bytes : %d \n", nr_bytes);
            #endif
            //JTAG
            digitalWrite(TCK_PIN, LOW);
            std::uint32_t bytesLeft = nr_bytes;
            std::uint32_t bitsLeft = len;
            std::uint32_t byteIndex = 0;
            std::uint8_t tdi, tms, tdo;

            while (bytesLeft > 0) {
                if (bytesLeft >= 2) {//倒数第二个完整8bit，最后一个数据可能不满8bit
                    //memcpy(&tms, &buffer[byteIndex], 1);
                    tms=buffer[byteIndex];
                    //memcpy(&tdi, &buffer[byteIndex + nr_bytes], 1);
                    tdi=buffer[byteIndex + nr_bytes];

                    tdo = jtag_xfer(8, tms, tdi);
                    //memcpy(&result[byteIndex], &tdo, 1);
                    result[byteIndex]=tdo;

                    bytesLeft -= 1;
                    bitsLeft -= 8;
                    byteIndex += 1;
                    #ifdef PRINT_LOG_UART
                    ESP_LOGD(TAG, "LEN : 0x%08x\n", 8);
                    ESP_LOGD(TAG, "TMS : 0x%08x\n", tms);
                    ESP_LOGD(TAG, "TDI : 0x%08x\n", tdi);
                    ESP_LOGD(TAG, "TDO : 0x%08x\n", tdo);
                    #endif
                } else {
                    //memcpy(&tms, &buffer[byteIndex], bytesLeft);
                    tms=buffer[byteIndex];
                    //memcpy(&tdi, &buffer[byteIndex + nr_bytes], bytesLeft);
                    tdi=buffer[byteIndex + nr_bytes];

                    tdo = jtag_xfer(bitsLeft, tms, tdi);
                    //memcpy(&result[byteIndex], &tdo, bytesLeft);
                    result[byteIndex]=tdo;

                    bytesLeft = 0;
                    #ifdef PRINT_LOG_UART
                    ESP_LOGD(TAG, "LEN : 0x%08x\n", bitsLeft);
                    ESP_LOGD(TAG, "TMS : 0x%08x\n", tms);
                    ESP_LOGD(TAG, "TDI : 0x%08x\n", tdi);
                    ESP_LOGD(TAG, "TDO : 0x%08x\n", tdo);
                    #endif
                    break;
                }
            }
            //JTAG
            digitalWrite(TCK_PIN, LOW);

            if (write(fd, result, nr_bytes) != nr_bytes) {
                #ifdef PRINT_LOG_UART
                ESP_LOGE(TAG, "write");
                #endif
                return false;
            }

            return true;
        }

        void run()
        {
            if (this->client_socket.is_valid()) {
                if (!this->handle_data()) {
                    this->client_socket.release();
                }
            }
            else {
                if (this->wait_connection()) {
                    // Nothing to do.
                }
            }
        }
};

#ifdef PRINT_LOG_UART
static void serialTask(void*)
{
    std::size_t pendingBytesH2T = 0;
    std::size_t bytesWrittenH2T = 0;
    static std::uint8_t h2tBuffer[128];
    std::size_t pendingBytesT2H = 0;
    std::size_t bytesWrittenT2H = 0;
    static std::uint8_t t2hBuffer[128];

    while (true) {
        if (Serial.available()) {
            Serial1.write(Serial.read());
        }
        else if (Serial1.available()) {
            Serial.write(Serial1.read());
        }
        else {
            vTaskDelay(pdMS_TO_TICKS(1));
        }
    }
}
#endif

bool AutoConfig()//自动连接
{
    //如果觉得时间太长可改
    const int loop = 14;
    for (int i = 0; i < loop; i++)
        display.drawString(i*7, 10, "_");
    for (int i = 0; i < loop; i++)
    {
            led_sta=!led_sta;
            digitalWrite(LED_blk, led_sta);
            display.drawString(i*7, 10, "*");
            display.display();
            if (WiFi.status() == WL_CONNECTED)
            {
                #ifdef PRINT_LOG_UART
                Serial.println("WIFI SmartConfig Success");
                Serial.printf("SSID:%s", WiFi.SSID().c_str());
                Serial.printf(", PSW:%s\r\n", WiFi.psk().c_str());
                Serial.print("LocalIP:");
                Serial.print(WiFi.localIP());
                Serial.print(" ,GateIP:");
                Serial.println(WiFi.gatewayIP());
                #endif
                return true;
            }
            else
            {
                #ifdef PRINT_LOG_UART
                Serial.print("WIFI AutoConfig Waiting......");
                #endif
                delay(1000);
            }
    }
    #ifdef PRINT_LOG_UART
    Serial.println("WIFI Connect Timeout!" );
    #endif
    display.clear();
    display.drawString(0, 0, "WIFI Connect Timeout!");
    display.drawString(0, 10, "Use SmartConfig.");
    display.display();
    return false;
}
void SmartConfig()//智能配网，用手机操作
{
    #ifdef PRINT_LOG_UART
    Serial.println("\r\nWait for Smartconfig...");
    #endif
    WiFi.beginSmartConfig();
    digitalWrite(LED_blk, HIGH);
    while (1)
    {
        led_sta=!led_sta;
        digitalWrite(LED_blk, led_sta);
        delay(334);                   // wait for a second
        if (WiFi.smartConfigDone())
        {
            #ifdef PRINT_LOG_UART
            Serial.println("SmartConfig Success");
            Serial.printf("SSID:%s\r\n", WiFi.SSID().c_str());
            Serial.printf("PSW:%s\r\n", WiFi.psk().c_str());
            #endif
            break;
        }
    }
}

//初始化代码
void setup()
{
    WiFi.begin();
    pinMode(LED_blk, OUTPUT);//初始化led
    digitalWrite(LED_blk, led_sta);//off
    display.init();//绘图
    display.drawString(0, 0, "xiaowuzxc/ESP32-XVC");
    display.display();
    jtag_init();
    #ifdef PRINT_LOG_UART
    Serial.begin(115200);
    #endif
    delay(1500);
    //等WIFI连接
    display.clear();
    display.drawString(0, 0, "Wait WIFI Auto Connect.");
    display.display();
    delay(1500);
    WiFi.reconnect();
    if (!AutoConfig())//开启自动配网
        SmartConfig();
    digitalWrite(LED_blk, LOW);//亮灯
    display.clear();
    display.drawString(0, 0, "WIFI Connect Success");
    display.drawString(0, 10, "IP:");
    display.drawString(16, 10, WiFi.localIP().toString().c_str());
    int value = XVC_Port_Num;
    String port_string = "Port:";
    port_string.concat(value);
    display.drawString(0, 20, port_string);
    display.display();
    TaskHandle_t handle;
}

enum class AppState
{
    WaitingAPConnection,
    APConnected,
    ClientConnected,
};

static std::unique_ptr<XvcServer> server;

void loop()
{
    static AppState state = AppState::WaitingAPConnection;
    switch (state) {
        case AppState::WaitingAPConnection: {
                if (WiFi.isConnected()) {
                    #ifdef PRINT_LOG_UART
                    ESP_LOGI(TAG, "WiFi connection ready. IP: %s", WiFi.localIP().toString().c_str());
                    Serial.print(WiFi.localIP());
                    #endif
                    state = AppState::APConnected;
                }
                break;
            }
        case AppState::APConnected: {
                if (!WiFi.isConnected()) {
                    #ifdef PRINT_LOG_UART
                    ESP_LOGI(TAG, "disconnected from WiFi.");
                    #endif
                    server.release();
                    state = AppState::WaitingAPConnection;
                    break;
                }

                if (!server) {
                    server.reset(new XvcServer(XVC_Port_Num));
                }
                if (server) {
                    server->run();
                }
                break;
            }
    }
}


/*
     This work, "xvc-esp32.ino", is a derivative of "xvcpi.c" (https://github.com/derekmulcahy/xvcpi)
     by Derek Mulcahy.

     "xvc-esp32.ino" is licensed under CC0 1.0 Universal (http://creativecommons.org/publicdomain/zero/1.0/)
     by Kenta IDA (fuga@fugafuga.org)

     The original license information of "xvcpi.c" is attached below.
*/

/*
     This work, "xvcpi.c", is a derivative of "xvcServer.c" (https://github.com/Xilinx/XilinxVirtualCable)
     by Avnet and is used by Xilinx for XAPP1251.

     "xvcServer.c" is licensed under CC0 1.0 Universal (http://creativecommons.org/publicdomain/zero/1.0/)
     by Avnet and is used by Xilinx for XAPP1251.

     "xvcServer.c", is a derivative of "xvcd.c" (https://github.com/tmbinc/xvcd)
     by tmbinc, used under CC0 1.0 Universal (http://creativecommons.org/publicdomain/zero/1.0/).

     Portions of "xvcpi.c" are derived from OpenOCD (http://openocd.org)

     "xvcpi.c" is licensed under CC0 1.0 Universal (http://creativecommons.org/publicdomain/zero/1.0/)
     by Derek Mulcahy.
*/
